#include <ros/ros.h>
#include <serial/serial.h>
#include <iostream>
#include "fdilink_decode.h"
#include "sensor_msgs/Imu.h"
#include "sensor_msgs/MagneticField.h"
#include <string>
#include <iomanip>

#include "tf2/LinearMath/Transform.h"
#include "tf2/convert.h"
#include "tf2/utils.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "tf2_ros/buffer.h"
#include "tf2_ros/message_filter.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2_ros/transform_listener.h"

extern "C" int InuptChar(uint8_t c);

extern FDILink_IMUData_Packet_t IMU_Recive;
extern Quaternion_Orientation_Packet_t Quaternion_Orientation_Recive;
extern Euler_Orientation_Packet_t Euler_Orientation_Recive;

ros::Publisher pub_imu; //imu msg publisher
ros::Publisher pub_mag; //magnetometer msg publisher

int main(int argc, char **argv) {
    std::cout << "========================" << std::endl
              << "Ros wrapper for FDI Sensor" << std::endl
              << "it contains a accelerameter, gyroscope and magnetometer" << std::endl
              << "========================" << std::endl;

    ros::init(argc, argv, "launch_imu_node");
    ros::NodeHandle nh("~");

    //read serial port param
    std::string IMU_SERIAL_PORT("/dev/ttyUSB0");

    /* //Get Param的三种方法
    //① ros::param::get()获取参数“param1”的value，写入到param1上
    bool test1 = ros::param::get("param1", param1);
    
    //② ros::NodeHandle::getParam()获取参数，与①作用相同
    bool test2 = nh.getParam("param2",param2);
    
    //③ ros::NodeHandle::param()类似于①和②
    //但如果get不到指定的param，它可以给param指定一个默认值(如1)
    nh.param("param3", param3, 1); */


    if (!nh.getParam("imu_serial_port", IMU_SERIAL_PORT)) {
        ROS_WARN_STREAM("The imu serial port not set, use default: /dev/ttyUSB0");
    }

    pub_imu = nh.advertise<sensor_msgs::Imu>("/fdi_imu", 1000);
    pub_mag = nh.advertise<sensor_msgs::MagneticField>("/fdi_mag", 1000);

    serial::Serial sp; //声明串口对象
    serial::Timeout to = serial::Timeout::simpleTimeout(100);
    sp.setPort(IMU_SERIAL_PORT); //此处需要根据imu的接的串口来进行修改
    sp.setBaudrate(921600);//波特率
    sp.setTimeout(to);
    try {
        sp.open();
    }
    catch (serial::IOException &e) {
        ROS_ERROR_STREAM(
                "Unable to open port " << IMU_SERIAL_PORT << ", may be a wrong port number or permission denied");
        return -1;
    }
    if (sp.isOpen()) //检测串口是否已经打开，并给出提示
    {
        ROS_INFO_STREAM(
                IMU_SERIAL_PORT << " is opened. ");

    } else {
        return -1;
    }
    ros::Rate loop_rate(100);
    while (ros::ok()) {
        size_t n = sp.available();
        if (n != 0) {
            uint8_t buffer[256];
            n = n > 256 ? 256 : n;
            n = sp.read(buffer, n);
            for (int i = 0; i < n; i++) {
                if (InuptChar(buffer[i]) == FDILINK_IMUDATA_PACKET_ID) {
                    // std::cout 
                    //   << "[" <<IMU_Recive.Accelerometer_X 
                    //   << "," <<IMU_Recive.Accelerometer_Y 
                    //   << "," <<IMU_Recive.Accelerometer_Z 
                    //   << "," <<IMU_Recive.Gyroscope_X
                    //   << "," <<IMU_Recive.Gyroscope_Y
                    //   << "," <<IMU_Recive.Gyroscope_Z
                    //   << "," <<IMU_Recive.Magnetometer_X
                    //   << "," <<IMU_Recive.Magnetometer_Y
                    //   << "," <<IMU_Recive.Magnetometer_Z
                    //   << "]" << std::endl;

                    //assemble ros msgs 
                    sensor_msgs::Imu imu_msg;
                    sensor_msgs::MagneticField mag_msg;
                    ros::Time time = ros::Time::now();
                    //double secs = time.toSec();
                    //std::cout << std::setprecision(20) << "shijian" << secs << std::endl;
                    imu_msg.header.stamp = time;
                    imu_msg.header.frame_id = "fdi_imu";
                    imu_msg.angular_velocity.x = IMU_Recive.Gyroscope_Y;
                    imu_msg.angular_velocity.y = IMU_Recive.Gyroscope_X;
                    imu_msg.angular_velocity.z = -IMU_Recive.Gyroscope_Z;
                    imu_msg.linear_acceleration.x = IMU_Recive.Accelerometer_Y;
                    imu_msg.linear_acceleration.y = IMU_Recive.Accelerometer_X;
                    imu_msg.linear_acceleration.z = -IMU_Recive.Accelerometer_Z;
                    //add orientation msg
                    tf2::Quaternion q;
                    q.setRPY(Euler_Orientation_Recive.Pitch, Euler_Orientation_Recive.Roll, -Euler_Orientation_Recive.Heading);
                    tf2::convert(q, imu_msg.orientation);
                     

                    //std::cout <<"quantion yaw is:"<< tf2::getYaw(imu_msg.orientation) * 57.3 << std::endl;
                    

                    mag_msg.header = imu_msg.header;
                    mag_msg.magnetic_field.x = IMU_Recive.Magnetometer_X;
                    mag_msg.magnetic_field.y = IMU_Recive.Magnetometer_Y;
                    mag_msg.magnetic_field.z = IMU_Recive.Magnetometer_Z;

                    pub_imu.publish(imu_msg);
                    pub_mag.publish(mag_msg);

                }
            }
        }
        loop_rate.sleep();
    }

    return 0;
}

